/****************************************************************************
 *  Copyright (C) 2019 RoboMaster.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 ***************************************************************************/
#ifndef ROBORTS_DECISION_BLACKBOARD_H
#define ROBORTS_DECISION_BLACKBOARD_H

#include <actionlib/client/simple_action_client.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <cmath>
#include "roborts_msgs/ArmorDetectionAction.h"
#include "roborts_msgs/GameResult.h"
#include "roborts_msgs/GameRobotBullet.h"
#include "roborts_msgs/GameRobotHP.h"
#include "roborts_msgs/GameStatus.h"
#include "roborts_msgs/GameZone.h"
#include "roborts_msgs/GameZoneArray.h"
#include "roborts_msgs/RobotDamage.h"
#include "roborts_msgs/RobotHeat.h"
#include "roborts_msgs/RobotShoot.h"
#include "roborts_msgs/RobotStatus.h"


#include "roborts_msgs/multi_car.h"
#include "roborts_msgs/single_car.h"


#include "roborts_msgs/visual_detection.h"
#include "roborts_msgs/GimbalAngle.h"
#include "roborts_costmap/Nlayer.h"
#include "roborts_msgs/GimbalFb.h"

#include "io/io.h"
#include "../proto/decision.pb.h"
#include "costmap/costmap_interface.h"
#include "std_msgs/Int16.h"
#include "std_msgs/Float64.h"
#include "std_msgs/Int8.h"
#include "std_msgs/Bool.h"
#include "std_msgs/Time.h"
#include "nav_msgs/Odometry.h"
#include <iostream>
#include <roborts_msgs/FricWhl.h>
#include "../../roborts_base/include/communication.h"

namespace roborts_decision{
    class IMUBuff
    {
        public:
            const int LEN = 5;
            std::deque<roborts_msgs::GimbalFb> imu_history;
            ros::Subscriber imu_sub;
            
            IMUBuff()
            {
                ros::NodeHandle imu_n_;
                imu_sub = imu_n_.subscribe("gimbal_feedback", 1, &IMUBuff::storeIMUHistory, this);
            }

            void storeIMUHistory(const roborts_msgs::GimbalFb::ConstPtr &fb)
            {
                //std::cout<<"aaaaaaaaaaaaaaaaaa"<<std::endl;
                while(imu_history.size() >= LEN) imu_history.pop_back();
                imu_history.push_front(*fb);
            }

            double getPredYaw(ros::Time& rostimeStamp)
            {
                int64_t timeStamp = rostimeStamp.toSec()*pow(10,6);
                int64_t post_time, pre_time;
                double post_yaw, pre_yaw;
                if(imu_history.size()<2)
                {
                    return imu_history[0].imu.yaw_angle;
                }
                if((imu_history[0].stamp).toSec()*pow(10,6) <= timeStamp)
                {
                    post_yaw = imu_history[0].imu.yaw_angle;
                    post_time = imu_history[0].stamp.toSec()*pow(10,6);
                    pre_yaw = imu_history[1].imu.yaw_angle;
                    pre_time = imu_history[1].stamp.toSec()*pow(10,6);
                }
                else
                {
                    for(auto it=imu_history.begin(); it!=imu_history.end(); it++)
                    {
                        if((it->stamp).toSec()*pow(10,6) >= timeStamp)
                        {
                            post_yaw = (*it).imu.yaw_angle;
                            post_time = (*it).stamp.toSec()*pow(10,6);
                        }
                        else
                        {
                            pre_yaw = (*it).imu.yaw_angle;
                            pre_time = (*it).stamp.toSec()*pow(10,6);
                            break;
                        }
                    }
                }
                double k = (post_yaw-pre_yaw)/double(post_time-pre_time);
                double pred_yaw = pre_yaw + k*(timeStamp-pre_time);
                int64_t last = imu_history[0].stamp.toSec()*pow(10,6);
                // std::cout<<"PRE YAW: "<<pre_yaw<<" , TIME: "<<pre_time<<"\n";
                // std::cout<<"POST YAW: "<<post_yaw<<" , TIME: "<<post_time<<"\n";
                // std::cout<<"k:"<<k<<std::endl;
                // std::cout<<"PREDICT YAW: " << pred_yaw<<" , TIME: "<<timeStamp<<"\n";
                return pred_yaw;
            }

            double getLatestYaw()
            {
                return imu_history[0].imu.yaw_angle;
            }

            float getPredPitch(int64_t timeStamp)
            {
                int64_t post_time, pre_time;
                float post_pitch, pre_pitch;
                if(imu_history.size()<2)
                {
                    return imu_history[0].imu.pitch_angle;
                }
                if((imu_history[0].stamp).toSec()*pow(10,6) <= timeStamp)
                {
                    post_pitch = imu_history[0].imu.pitch_angle;
                    post_time = imu_history[0].stamp.toSec()*pow(10,6);
                    pre_pitch = imu_history[1].imu.pitch_angle;
                    pre_time = imu_history[1].stamp.toSec()*pow(10,6);
                }
                else
                {
                    for(auto it=imu_history.begin(); it!=imu_history.end(); it++)
                    {
                        if((it->stamp).toSec()*pow(10,6) >= timeStamp)
                        {
                            post_pitch = (*it).imu.pitch_angle;
                            post_time = (*it).stamp.toSec()*pow(10,6);
                        }
                        else
                        {
                            pre_pitch = (*it).imu.pitch_angle;
                            pre_time = (*it).stamp.toSec()*pow(10,6);
                            break;
                        }
                    }
                }
                float k = (post_pitch-pre_pitch)/float(post_time-pre_time);
                return pre_pitch + k*(timeStamp-pre_time);
            }
    };

class Blackboard{
public:
  typedef std::shared_ptr<Blackboard> Ptr;
  typedef roborts_costmap::CostmapInterface CostMap;
  typedef roborts_costmap::Costmap2D CostMap2D;
  explicit Blackboard(const std::string &proto_file_path):
    enemy_detected_(false),
    armor_detection_actionlib_client_("armor_detection_node_action", true){

      tf_ptr_ = std::make_shared<tf::TransformListener>(ros::Duration(10));

      std::string map_path = ros::package::getPath("roborts_costmap") + \
        "/config/costmap_parameter_config_for_decision.prototxt";
      costmap_ptr_ = std::make_shared<CostMap>("decision_costmap", *tf_ptr_, map_path);
      charmap_ = costmap_ptr_->GetCostMap()->GetCharMap();

      costmap_2d_ = costmap_ptr_->GetLayeredCostmap()->GetCostMap();

      if (!LoadBuffParam(proto_file_path)) {
        //std::cout<<proto_file_path<<std::endl;
        ROS_ERROR("%s can't open file", __FUNCTION__);
      }

      imu_buff = new IMUBuff();
      // Enemy fake pose
      ros::NodeHandle rviz_nh("/move_base_simple");
      enemy_sub_ = rviz_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, &Blackboard::GoalCallback, this);

      ros::NodeHandle nh;
      game_result_sub_ = nh.subscribe<roborts_msgs::GameResult>("/game_result",1,&Blackboard::GameResultCallback, this);
      game_robot_bullet_sub_ = nh.subscribe<roborts_msgs::GameRobotBullet>("/game_robot_bullet",1,&Blackboard::GameRobotBulletCallback, this);
      game_robot_hp_sub_ = nh.subscribe<roborts_msgs::GameRobotHP>("/game_robot_hp",1,&Blackboard::GameRobotHPCallback, this);
      game_status_sub_ = nh.subscribe<roborts_msgs::GameStatus>("/game_status",1,&Blackboard::GameStatusCallback, this);
      game_zone_array_status_sub_ = nh.subscribe<roborts_msgs::GameZoneArray>("/game_zone_array_status",1,&Blackboard::GameZoneArrayCallback, this);
      robot_damage_sub_ = nh.subscribe<roborts_msgs::RobotDamage>("/robot_damage",1,&Blackboard::RobotDamageCallback, this);
      robot_heat_sub_ = nh.subscribe<roborts_msgs::RobotHeat>("/robot_heat",1,&Blackboard::RobotHeatCallback, this);
      robot_shoot_sub_ = nh.subscribe<roborts_msgs::RobotShoot>("/robot_shoot",1,&Blackboard::RobotShootCallback, this);
      robot_status_sub_ = nh.subscribe<roborts_msgs::RobotStatus>("/robot_status",1,&Blackboard::RobotStatusCallback, this);
      
      cmd_vel_sub_ = nh.subscribe<geometry_msgs::Twist>("cmd_vel",1,&Blackboard::CmdVelCallback, this);
      odom_sub_ = nh.subscribe<nav_msgs::Odometry>("odom",1,&Blackboard::OdomCallback, this);
      scan_sub_ = nh.subscribe<sensor_msgs::LaserScan>("scan",1,&Blackboard::ScanCallback, this);
      global_costmap_sub_ = nh.subscribe<nav_msgs::OccupancyGrid>("global_costmap/global_costmap/costmap",1,&Blackboard::costmapCallback, this);
      gimbal_angle_sub_=nh.subscribe<std_msgs::Int16>("gimbal_gyro",10,&Blackboard::GimbalAngleCallback, this);//云台对地角度,陀螺仪
      aim_target_pub_ = nh.advertise<geometry_msgs::PoseStamped>("aim_target",1);
      visual_detection_sub_=nh.subscribe<roborts_msgs::visual_detection>("/roborts_all",10,&Blackboard::VisualDetectionCallback,this);
      current_buff_open_pub_ = nh.advertise<roborts_costmap::Nlayer>("current_buff_open_info",10);
      
      shoot_client = nh.serviceClient<roborts_msgs::FricWhl>("cmd_fric_wheel");
    
      robot_send_pub_ = nh.advertise<roborts_msgs::multi_car>("/robot_send",1);
      robot_recv_sub_ = nh.subscribe<roborts_msgs::multi_car>("/robot_recv",1,&Blackboard::TCPPoseCallback,this);
      camera_sub_ = nh.subscribe<roborts_msgs::multi_car>("/camera_recv",1,&Blackboard::CameraCallback,this);
      

      //Air Block
      air_block_pub_=nh.advertise<geometry_msgs::PoseStamped>("air_block", 1);
      prepare_action_=1;

      roborts_common::ReadProtoFromTextFile(proto_file_path, &decision_config);

      if (!decision_config.simulate()){
        armor_detection_actionlib_client_.waitForServer();

        ROS_INFO("Armor detection module has been connected!");

        armor_detection_goal_.command = 1;
        armor_detection_actionlib_client_.sendGoal(armor_detection_goal_,
                                                actionlib::SimpleActionClient<roborts_msgs::ArmorDetectionAction>::SimpleDoneCallback(),
                                                actionlib::SimpleActionClient<roborts_msgs::ArmorDetectionAction>::SimpleActiveCallback(),
                                                boost::bind(&Blackboard::ArmorDetectionFeedbackCallback, this, _1));
      }
      
      for(int i=0;i<4;i++) {
        roborts_msgs::single_car new_car;
        tcp_car_info_.multicar.emplace_back(new_car);
      }
      tcp_car_info_.multicar[0].id = 1;
      tcp_car_info_.multicar[0].color = 1;
      tcp_car_info_.multicar[1].id = 2;
      tcp_car_info_.multicar[1].color = 1;
      tcp_car_info_.multicar[2].id = 1;
      tcp_car_info_.multicar[2].color = 2;
      tcp_car_info_.multicar[3].id = 2;
      tcp_car_info_.multicar[3].color = 2;
    }

  ~Blackboard() = default;

  // Enemy
  void ArmorDetectionFeedbackCallback(const roborts_msgs::ArmorDetectionFeedbackConstPtr& feedback){
    if (feedback->detected){
      enemy_detected_ = true;
      ROS_INFO("Find Enemy!");

      tf::Stamped<tf::Pose> tf_pose, global_tf_pose;
      geometry_msgs::PoseStamped camera_pose_msg, global_pose_msg;
      camera_pose_msg = feedback->enemy_pos;

      double distance = std::sqrt(camera_pose_msg.pose.position.x * camera_pose_msg.pose.position.x +
        camera_pose_msg.pose.position.y * camera_pose_msg.pose.position.y);
      double yaw = atan(camera_pose_msg.pose.position.y / camera_pose_msg.pose.position.x);

      //camera_pose_msg.pose.position.z=camera_pose_msg.pose.position.z;
      tf::Quaternion quaternion = tf::createQuaternionFromRPY(0,
                                                              0,
                                                              yaw);
      camera_pose_msg.pose.orientation.w = quaternion.w();
      camera_pose_msg.pose.orientation.x = quaternion.x();
      camera_pose_msg.pose.orientation.y = quaternion.y();
      camera_pose_msg.pose.orientation.z = quaternion.z();
      poseStampedMsgToTF(camera_pose_msg, tf_pose);

      tf_pose.stamp_ = ros::Time(0);
      try
      {
        tf_ptr_->transformPose("map", tf_pose, global_tf_pose);
        tf::poseStampedTFToMsg(global_tf_pose, global_pose_msg);

        if(GetDistance(global_pose_msg, enemy_pose_)>0.2 || GetAngle(global_pose_msg, enemy_pose_) > 0.2){
          enemy_pose_ = global_pose_msg;
        }
      }
      catch (tf::TransformException &ex) {
        ROS_ERROR("tf error when transform enemy pose from camera to map");
      }
    }
    else{
      enemy_detected_ = false;
    }
  }

  bool isOccupied()
  {
      auto start_pose = GetRobotMapPose();
      unsigned int start_pose_mx,start_pose_my;
      if(!costmap_2d_->World2Map(start_pose.pose.position.x,start_pose.pose.position.y,start_pose_mx,start_pose_my))
      {
	        return false;
      }
      if(costmap_2d_->GetCost(start_pose_mx,start_pose_my)>253)
      {
          return false;
      }
      return true;
  }

  geometry_msgs::PoseStamped GetEnemy() const {
    return enemy_pose_;
  }

  bool ifCurrentValid(){    //whether current position is free
    auto cur = GetRobotMapPose();
    unsigned int mx,my;
    costmap_2d_->World2Map(cur.pose.position.x,cur.pose.position.y,mx,my);
    if(global_costmap_.data.size() == 0)
    {
      return true;
    }
    auto cost = global_costmap_.data[costmap_2d_->GetIndex(mx,my)];
    std::cout<<"current_cost:"<<int(cost)<<std::endl;
    return cost<10;
  }

  bool IsEnemyDetected() const{
    ROS_INFO("%s: %d", __FUNCTION__, (int)enemy_detected_);
    return enemy_detected_;
  }

  // Goal
  void GoalCallback(const geometry_msgs::PoseStamped::ConstPtr& goal){
    new_goal_ = true;
    goal_ = *goal;
  }

  geometry_msgs::PoseStamped GetGoal() const {
    return goal_;
  }

  bool IsNewGoal(){
    if(new_goal_){
      new_goal_ =  false;
      return true;
    } 
    else{
      return false;
    }
  }
 
/*----------------------------------Add Some Function---------------------------------------*/
  roborts_decision::DecisionConfig GetDecisionConfig()
  {return decision_config;}

  int GetMyId() {
    return my_id;
  }
  
  int GetMyColor() {
    return my_color;
  }

  void TurnMyId(int id) {
    if(id == 1 || id == 2) {
        my_id = id;
        my_color = 1;
    }
    if(id == 3 || id == 4) {
        my_id = id;
        my_color = 2;
    }
  }

  double GetGimbalEncoder() {
    double yaw,pitch,roll;
    try {
      tf::StampedTransform transform;
      tf_ptr_->waitForTransform("base_link" ,"gimbal",  ros::Time(0),ros::Duration(3.0));  
      tf_ptr_->lookupTransform("base_link" ,"gimbal",ros::Time(0),transform);  
      tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("tf error when transform enemy pose from base_link to gimbal");
    }
      return yaw;
  }

  bool isCircle()
  {
    if(ros::Time::now()-goal_.header.stamp>ros::Duration(3.0))
    {
      return false;
    }
    else
      return true;
  }

 /*----------------------------------Add Some Information---------------------------------------*/

  void CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_vel) {
    current_cmd_vel_ = *cmd_vel;
  }

  const geometry_msgs::Twist GetCmdVelinfo() {
    //ROS_INFO("Getting Cmd_vel info.");
    return current_cmd_vel_;
  }

  void OdomCallback(const nav_msgs::Odometry::ConstPtr& odom) {
    odom_ = *odom;
  }
  
  const nav_msgs::Odometry GetOdominfo() {
    return odom_;
  }

  void ScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
    ShareMytcpcarinfo();// 双车通信
    scan_ = *scan;
  }

  sensor_msgs::LaserScan GetScan() {
    return scan_;
  }

  void GimbalAngleCallback(const std_msgs::Int16::ConstPtr& gimbal_angle){//陀螺仪
   yaw_angle_ = gimbal_angle->data/1800.0*3.14159265;
  }

  double GetGimbalAngle() const{
   return yaw_angle_;
  }
    
  void costmapCallback(const nav_msgs::OccupancyGrid::ConstPtr& map)
  {
    if(ros::Time::now() - global_costmap_.header.stamp>ros::Duration(5.0))
    {
      global_costmap_ = *map;
    }
  }

  nav_msgs::OccupancyGrid* GetGlobalCostMap()
  {
    return &global_costmap_;
  }
    
 /*-----------------------------------Decision Condition---------------------------------------*/

bool PrepareCondition()
{
  if(ros::Time::now().sec-prepare_time_.sec>5&&GetIfEnemy())return true;
  else return false;
}

bool RecoveryCondition()
{
  for(int i=0;i<scan_.ranges.size();i++)
  {
    if (scan_.ranges[i]<0.3){std::cout<<scan_.ranges[i]<<' '; return 1;}
  }
  return 0;
}

bool cover_condition()
{
  if(IsTCPWork() == false) return false;
  if(getfriendpose().header.stamp.sec-GetNewEnemyCar().header.stamp.sec<1&&getfriendpose().header.stamp.sec!=0&&GetNewEnemyCar().header.stamp.sec!=0)return true;
  else return false;
}

  bool start_condition() {
    roborts_msgs::FricWhl Fric;
    Fric.request.open = true;
    shoot_client.call(Fric);
    if(game_status_.game_status == 4) {
      //std::cout<<"game start"<<std::endl;
      return true;
    }
    else
      return false;
  }

  bool attack_condition() {
    if(IsBelowShootHeat() && GetIfShoot()) {
      std::cout<<"attack_condition true"<<std::endl;
      return true;
    }
    else return false;
  }
  
  bool chase_condition() {  // 敌人在2.5周围,可能会处于追击和巡逻的犹豫状态?
    if(attack_enemy_buff_condition()) return false;
    if(GetIfEnemy() && GetDistance(GetRobotMapPose(), GetAimPosition()) > 2.5)
      {
        chase_condition_track_=1;
        std::cout<<"chase1\n";
        return true;
      }
    else if(GetIfEnemy() && GetDistance(GetRobotMapPose(), GetAimPosition()) > 2&&chase_condition_track_)
    {
      chase_condition_track_=1;
      std::cout<<"chase2\n";
      return true;
    }
    else if(GetIfEnemy() && GetDistance(GetRobotMapPose(), GetAimPosition()) > 2&&!chase_condition_track_)
    {
      chase_condition_track_=0;
      std::cout<<"chase3\n";
      return false;
    }
    else 
      {
        chase_condition_track_=0;
        std::cout<<"chase4\n";
        return false;
      }
  }
  
  bool circle_condition() {
    if(GetMyHPinfo() < 200)
      return true;
    else 
      return false;
  }
  
  bool blood_add_condition() {
    if(GetMyHpRecoveryBuffInfo() && GetMyHPinfo()<1800)
      return true;
    else return false;
  }

  bool attack_enemy_buff_condition() {
    if(GetEnemyBulletSupplyBuffInfo() == 0) return false;
    int time = GetGameRemainTime();
    time = time % 60;
    if(time < 40) return false;
    /*if(GetMyColor() == 1) {
      if(GetEnemyBulletSupplyBuffInfo() == 1||GetEnemyBulletSupplyBuffInfo() == 2)
        return false;
    }
    else if(GetMyColor() == 2) {
      if(GetEnemyBulletSupplyBuffInfo() == 5||GetEnemyBulletSupplyBuffInfo() == 6)
        return false;
    }*/
    return true;
  }
  
  bool IsBelowShootHeat() {
    if(robot_heat_.shooter_heat < 160)//max = 240 
      return true;
    else 
      return false;
  }

  bool IsBulletEnough() {
    switch(my_id){
      case 1:  if(game_robot_bullet_.red1>1)  return true;  break;
      case 2:  if(game_robot_bullet_.red2>1)  return true;  break;
      case 3:  if(game_robot_bullet_.blue1>1)  return true;  break;
      case 4:  if(game_robot_bullet_.blue2>1)  return true;  break;
      default:  break;
    }
    return false;
  }

  int GotoBulletBuffCondition() {
    int buff_num = GetMyBulletSupplyBuffInfo();
    if(buff_num == 0) return 0;
    if(GetFriendHPinfo() == 0) return buff_num;
    auto my_distance = GetDistance(GetRobotMapPose(),buff_position_[buff_num-1]);
    auto my_partner_distance = GetDistance(getfriendpose(),buff_position_[buff_num-1]);
    if(my_distance > my_partner_distance) return 0;
    else return buff_num; 
  }

  int GotoHpBuffCondition() {
    int buff_num = GetMyHpRecoveryBuffInfo();
    if(buff_num == 0) return 0;
    if(GetFriendHPinfo() == 0) return buff_num;
    auto my_distance = GetDistance(GetRobotMapPose(),buff_position_[buff_num-1]);
    auto my_partner_distance = GetDistance(getfriendpose(),buff_position_[buff_num-1]);
    if(my_distance > my_partner_distance) return 0;
    else return buff_num;
  }

/*----------------------------------visual detection---------------------------------------*/
  
  void VisualDetectionCallback(const roborts_msgs::visual_detection::ConstPtr& mess_all) {
    if_shoot_ = mess_all->if_shoot;
    if_enemy_ = mess_all->if_enemy;
    int length = mess_all->multicar.size();
    if(length == 0) return;
    mess_all_ = *mess_all;
    gimbalangle_ = mess_all_.G_angle;
    
    for(int i = 0;i<length;i++){
      if(mess_all_.multicar[i].pose.pose.orientation.w == 0.0)  return;
      if(mess_all_.multicar[i].pose.pose.position.x == 0.0&&mess_all_.multicar[i].pose.pose.position.y == 0.0&&mess_all_.multicar[i].pose.pose.position.z == 0.0) return;
      mess_all_.multicar[i].pose.pose.position.x= mess_all_.multicar[i].pose.pose.position.z/1000.0;
      mess_all_.multicar[i].pose.pose.position.y= -mess_all_.multicar[i].pose.pose.position.x/1000.0;
      mess_all_.multicar[i].pose.pose.position.z= mess_all_.multicar[i].pose.pose.position.y/1000.0;
      try {
        ros::Time tmp =  mess_all_.multicar[i].pose.header.stamp;
        mess_all_.multicar[i].pose.header.stamp = ros::Time(0);
        tf_ptr_->transformPose("map", mess_all_.multicar[i].pose, mess_all_.multicar[i].pose);
        mess_all_.multicar[i].pose.header.stamp = tmp;
        std::cout<<mess_all_.multicar[i].pose<<std::endl;
        tcpSinglecarUpdate(mess_all_.multicar[i]);
      }
      catch(tf::LookupException& ex) {
        std::cout<<"transform error"<<std::endl;
        //LOG_ERROR << "transform error, " << ex.what();
      }
    }
    aim_position_ = mess_all_.multicar[0].pose;

    aim_position_.header.stamp = ros::Time(0);
    tf_ptr_->transformPose("map", aim_position_, aim_position_);
    aim_target_pub_.publish(aim_position_);

  }

  roborts_msgs::visual_detection GetTarget() const{
    return mess_all_;
  }

  roborts_msgs::GimbalAngle GetGangle() const{
    return gimbalangle_;
  }

  geometry_msgs::PoseStamped GetAimPosition() const{
    return aim_position_;
  }

  bool GetIfShoot() const{
    return if_shoot_;
  }

  bool GetIfEnemy() const{
    return if_enemy_;
  }

  bool GetIfFindEnemy() {  
    if(if_enemy_ == 1) {
      last_find_time_.data = ros::Time::now();
      return true;
    }
    else if(ros::Time::now()-last_find_time_.data<ros::Duration(0.2))
      return true;
    else return false;
  }

  double getCurrentIMU(ros::Time& time)
  {
    return imu_buff->getPredYaw(time);
  }


  /*---------------------------------- Referee System ------------------------------------------*/

  void GameResultCallback(const roborts_msgs::GameResult::ConstPtr& game_result) {
    game_result_  = *game_result;
  }

  const roborts_msgs::GameResult GetGameResultinfo() {
    return game_result_;
  }

  void GameRobotBulletCallback(const roborts_msgs::GameRobotBullet::ConstPtr& game_robot_bullet) {
    game_robot_bullet_  = *game_robot_bullet;
  }

  const roborts_msgs::GameRobotBullet GetGameRobotBulletinfo() {
    return game_robot_bullet_;
  }
  
  void GameRobotHPCallback(const roborts_msgs::GameRobotHP::ConstPtr& game_robot_hp) {
    game_robot_hp_ = *game_robot_hp;
  }
  
  const int GetFriendHPinfo() {
    switch (my_id){
      case 1: return game_robot_hp_.red2;break;
      case 2: return game_robot_hp_.red1;break;
      case 3: return game_robot_hp_.blue2;break;
      case 4: return game_robot_hp_.blue1;break;
      default: break;
    }
    return 0;
  }

  void GameStatusCallback(const roborts_msgs::GameStatus::ConstPtr& game_status) {
    game_status_  = *game_status;
  }

  const roborts_msgs::GameStatus GetGameStatusinfo() {
    return game_status_;
  }

  int GetGameRemainTime() {
    return game_status_.remaining_time;
  }

  void GameZoneArrayCallback(const roborts_msgs::GameZoneArray::ConstPtr& game_zone_array_status) {
    game_zone_array_status_  = *game_zone_array_status;
    GetBuff();
  }

  const roborts_msgs::GameZoneArray GetGameZoneArrayinfo() {
    return game_zone_array_status_;//数组,获得六个buff区的状态
  }
  
  void RobotDamageCallback(const roborts_msgs::RobotDamage::ConstPtr& robot_damage) {
    robot_damage_  = *robot_damage;
    prepare_action_=1;
    prepare_time_=ros::Time::now();
    enemy_attackme();
  }

  const roborts_msgs::RobotDamage GetRobotDamageinfo() {
    return robot_damage_;
  }

  void RobotHeatCallback(const roborts_msgs::RobotHeat::ConstPtr& robot_heat) {
    robot_heat_  = *robot_heat;
  }

  const roborts_msgs::RobotHeat GetRobotHeatinfo() {
    return robot_heat_;
  }

  void RobotShootCallback(const roborts_msgs::RobotShoot::ConstPtr& robot_shoot) {
    robot_shoot_  = *robot_shoot;
  }

  const roborts_msgs::RobotShoot GetRobotShootinfo() {
    return robot_shoot_;
  }

  void RobotStatusCallback(const roborts_msgs::RobotStatus::ConstPtr& robot_status) {
    robot_status_  = *robot_status;
    TurnMyId(robot_status_.id/50+robot_status_.id%100);
  }

  const roborts_msgs::RobotStatus GetRobotStatusinfo() {
    return robot_status_;
  }
  
  const int GetMyHPinfo() {
	std::cout<<"robot_status_.remain_hp:"<<robot_status_.remain_hp<<std::endl;
    return robot_status_.remain_hp;
  }

  /*---------------------------------- Under Attack ------------------------------------------*/

  void enemy_attackme(){
    float bias=0;
    if(robot_damage_.damage_type==0){//ARMOR 正式使用时将该if取消注释
      under_attack_sign = true;
      attacked_odom_ = GetOdominfo();
      switch(robot_damage_.damage_source){
        case 0://FORWARD
          bias=0;
          break;
        case 1://LEFT	
          bias=1.57;
          break;
        case 2://BACKWARD
          bias=3.14;
          break;
        case 3://RIGHT
          bias=-1.57;
          break;
        default:
          break;
      }
      enemy_attack_me.data = bias;
    }
  }
  
  const std_msgs::Float64 GetAttackBiasinfo() {
    return enemy_attack_me;
  }

  const nav_msgs::Odometry GetAttackedOdominfo() {
    return attacked_odom_;
  }

  bool IsUnderAttack() {
    return under_attack_sign;
  }

  void TurnUnderAttackSign(bool sign) {
    under_attack_sign = sign;
  }

  void TurnPrepareSige(bool sign)
  {
    prepare_action_=sign;
  }

  /*---------------------------------- Buff -------------------------------------------------*/

  void GetBuff() {
    for(int i=0;i<6;i++) {
      auto type = game_zone_array_status_.zone[i].type;
      buffopeninfo[type-1] = game_zone_array_status_.zone[i].active;
      buffpositioninfo[type-1] = i + 1;
    }
    buffpub();
  }  

  bool* GetBuffOpenInfo() {
    return buffopeninfo;
  }

  int GetMyHpRecoveryBuffInfo() {
    if(my_id == 1||my_id == 2) {
      if(buffopeninfo[0])  return buffpositioninfo[0];
      else return 0;
    }
    else if(my_id == 3||my_id == 4) {
      if(buffopeninfo[2])  return buffpositioninfo[2];
      else return 0;
    }
  }

  int GetMyBulletSupplyBuffInfo() {
    if(my_id == 1||my_id == 2) {
      if(buffopeninfo[1])  return buffpositioninfo[1];
      else return 0;
    }
    else if(my_id == 3||my_id == 4) {
      if(buffopeninfo[3])  return buffpositioninfo[3];
      else return 0;
    }
  }
  
  int GetEnemyBulletSupplyBuffInfo() {
    if(my_id == 1||my_id == 2) {
      if(buffopeninfo[3])  return buffpositioninfo[3];
      else return 0;
    }
    else if(my_id == 3||my_id == 4) {
      if(buffopeninfo[1])  return buffpositioninfo[1];
      else return 0;
    }
  }

  void buffpub() {
    for(int i=0;i<6;i++) {
      buff_open_pub_info.derail[i] = 0;
    }
    if(buffopeninfo[4])  buff_open_pub_info.derail[buffpositioninfo[4]-1] = 1;
    if(buffopeninfo[5])  buff_open_pub_info.derail[buffpositioninfo[5]-1] = 1;
    if(my_id == 1||my_id == 2) {
      if(buffopeninfo[2])  buff_open_pub_info.derail[buffpositioninfo[2]-1] = 1;
      if(buffopeninfo[3])  buff_open_pub_info.derail[buffpositioninfo[3]-1] = 1;
    }
    if(my_id == 3||my_id == 4) {
      if(buffopeninfo[0])  buff_open_pub_info.derail[buffpositioninfo[0]-1] = 1;
      if(buffopeninfo[1])  buff_open_pub_info.derail[buffpositioninfo[1]-1] = 1;
    }     
    current_buff_open_pub_.publish(buff_open_pub_info);
  }

  bool LoadBuffParam(const std::string &proto_file_path) {
    roborts_decision::DecisionConfig decision_config;
    if (!roborts_common::ReadProtoFromTextFile(proto_file_path, &decision_config)) {
      return false;
    }
    for(int i=0;i<6;i++) {
      buff_position_[i].header.frame_id="map";
      buff_position_[i].pose.position.x=decision_config.buff_point(i).x();
      buff_position_[i].pose.position.y=decision_config.buff_point(i).y();
      buff_position_[i].pose.position.z=decision_config.buff_point(i).z();
      auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.buff_point(i).roll(),
                                                              decision_config.buff_point(i).pitch(),
                                                              decision_config.buff_point(i).yaw());
      buff_position_[i].pose.orientation = quaternion;
    }
    return true;
  }

  geometry_msgs::PoseStamped getbuffpositionfromblackboard(int i) {
    return buff_position_[i-1];
  }

 /*---------------------------------- Camera---------------------------------------------------*/

  void CameraCallback(const roborts_msgs::multi_car::ConstPtr& camera_info) {
    camera_info_ = *camera_info;
    camera_find_time_.data = ros::Time::now();  
  }

  bool IsCameraFindEnemy() {
    int length = 0;
    length = camera_info_.multicar.size();
    if(length == 0) return false;
    if(ros::Time::now() - camera_find_time_.data > ros::Duration(5.0)) return false;
    double near_distance = 1000;
    int enemy_num = 0;
    for(int i=0;i<length;i++) {
      if(camera_info_.multicar[i].color != my_color){
        enemy_num++;
        double now_enemy_distance = GetDistance(camera_info_.multicar[i].pose,GetRobotMapPose());
        if(now_enemy_distance < near_distance) {
          near_posestamped = camera_info_.multicar[i].pose;
          near_distance = now_enemy_distance;
        }
      }
    }
    if(enemy_num == 0) return false;
    std::cout<<"near_distance:"<<near_distance<<std::endl;
    if(near_distance < 0.5) return false;//0.5参数待修改
    return true; 
  }

  const geometry_msgs::PoseStamped GetEnemyPoseFromCamera() {
    auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(0,0,0);
    near_posestamped.pose.orientation = quaternion;//保证四元数不全为0
    return near_posestamped;
  }
  
  /*---------------------------------- communication ------------------------------------------------*/ 
  
  void sendmypose() {
    roborts_msgs::single_car my_single_carinfo;
    switch (my_id){
      case 1:
        my_single_carinfo.id = 1;
        my_single_carinfo.color = 1;
        break;
      case 2:
        my_single_carinfo.id = 2;
        my_single_carinfo.color = 1;
        break;
      case 3:
        my_single_carinfo.id = 1;
        my_single_carinfo.color = 2;
        break;
      case 4:
        my_single_carinfo.id = 2;
        my_single_carinfo.color = 2;
        break;
      default:
        break;
    }
    my_single_carinfo.pose = GetRobotMapPose();
    tcpSinglecarUpdate(my_single_carinfo);
  }
  
  void ShareMytcpcarinfo() {
    sendmypose();
    robot_send_pub_.publish(tcp_car_info_);
  }
  
  void TCPPoseCallback(const roborts_msgs::multi_car::ConstPtr& tcp_car_info) {
    tcpMulticarUpdate(*tcp_car_info);
    last_tcp_time_.data = ros::Time::now();
    air_block_pub_.publish(getfriendpose());
  }
  
  bool IsTCPWork() {
    if(ros::Time::now() - last_tcp_time_.data > ros::Duration(5.0)) return false;
    else return true;
  }
  
  geometry_msgs::PoseStamped getfriendpose() {
    geometry_msgs::PoseStamped friendpose;
    switch (my_id){
      case 1:
        friendpose = tcp_car_info_.multicar[1].pose;
        break;
      case 2:
        friendpose = tcp_car_info_.multicar[0].pose;
        break;
      case 3:
        friendpose = tcp_car_info_.multicar[3].pose;
        break;
      case 4:
        friendpose = tcp_car_info_.multicar[2].pose;
        break;
      default:
        break;
    }
    return friendpose;
  }
  
  geometry_msgs::PoseStamped getenemy1pose() {
    if(my_color == 1) return tcp_car_info_.multicar[2].pose;
    if(my_color == 2) return tcp_car_info_.multicar[0].pose;
  }
  
  geometry_msgs::PoseStamped getenemy2pose() {
    if(my_color == 1) return tcp_car_info_.multicar[3].pose;
    if(my_color == 2) return tcp_car_info_.multicar[1].pose;
  }
  
  geometry_msgs::PoseStamped getnearestenemypose() {
    geometry_msgs::PoseStamped enemypose1 = getenemy1pose();
    geometry_msgs::PoseStamped enemypose2 = getenemy2pose();
    double enemydistance1 = GetDistance(GetRobotMapPose(),enemypose1);
    double enemydistance2 = GetDistance(GetRobotMapPose(),enemypose2);
    if (enemydistance1 > enemydistance2)
      return enemypose2;
    else return enemypose1;
  }
  
  geometry_msgs::PoseStamped GetNewEnemyCar() {
    if(getenemy1pose().header.stamp < getenemy2pose().header.stamp) return getenemy2pose();
    else return getenemy1pose();
  }
  
  void tcpSinglecarUpdate(roborts_msgs::single_car car_info) {
    if(car_info.id == 1 && car_info.color == 1) {
      if(car_info.pose.header.stamp > tcp_car_info_.multicar[0].pose.header.stamp)
        tcp_car_info_.multicar[0].pose = car_info.pose;
      return;
    }
    if(car_info.id == 2 && car_info.color == 1) {
      if(car_info.pose.header.stamp > tcp_car_info_.multicar[1].pose.header.stamp)
        tcp_car_info_.multicar[1].pose = car_info.pose;
      return;
    }
    if(car_info.id == 1 && car_info.color == 2) {
      if(car_info.pose.header.stamp > tcp_car_info_.multicar[2].pose.header.stamp)
        tcp_car_info_.multicar[2].pose = car_info.pose;
      return;
    }
    if(car_info.id == 2 && car_info.color == 2) {
      if(car_info.pose.header.stamp > tcp_car_info_.multicar[3].pose.header.stamp)
        tcp_car_info_.multicar[3].pose = car_info.pose;
      return;
    }
  }
  
  void tcpMulticarUpdate(roborts_msgs::multi_car car_info) {
    int length = car_info.multicar.size();
    if(length == 0) return;  
    for(int i = 0;i<length;i++){
      if(car_info.multicar[i].id == 1 && car_info.multicar[i].color == 1) {
        if(car_info.multicar[i].pose.header.stamp > tcp_car_info_.multicar[0].pose.header.stamp)
          tcp_car_info_.multicar[0].pose = car_info.multicar[i].pose;
        continue;
      }
      if(car_info.multicar[i].id == 2 && car_info.multicar[i].color == 1) {
        if(car_info.multicar[i].pose.header.stamp > tcp_car_info_.multicar[1].pose.header.stamp)
          tcp_car_info_.multicar[1].pose = car_info.multicar[i].pose;
        continue;
      }
      if(car_info.multicar[i].id == 1 && car_info.multicar[i].color == 2) {
        if(car_info.multicar[i].pose.header.stamp > tcp_car_info_.multicar[2].pose.header.stamp)
          tcp_car_info_.multicar[2].pose = car_info.multicar[i].pose;
        continue;
      }
      if(car_info.multicar[i].id == 2 && car_info.multicar[i].color == 2) {
        if(car_info.multicar[i].pose.header.stamp > tcp_car_info_.multicar[3].pose.header.stamp)
          tcp_car_info_.multicar[3].pose = car_info.multicar[i].pose;
        continue;
      }
    }
  }
  
  /*---------------------------------- Tools ------------------------------------------------*/

  double GetDistance(const geometry_msgs::PoseStamped &pose1,
                     const geometry_msgs::PoseStamped &pose2) {
    const geometry_msgs::Point point1 = pose1.pose.position;
    const geometry_msgs::Point point2 = pose2.pose.position;
    const double dx = point1.x - point2.x;
    const double dy = point1.y - point2.y;
    return std::sqrt(dx * dx + dy * dy);
  }

  double GetAngle(const geometry_msgs::PoseStamped &pose1,
                  const geometry_msgs::PoseStamped &pose2) {
    const geometry_msgs::Quaternion quaternion1 = pose1.pose.orientation;
    const geometry_msgs::Quaternion quaternion2 = pose2.pose.orientation;
    tf::Quaternion rot1, rot2;
    tf::quaternionMsgToTF(quaternion1, rot1);
    tf::quaternionMsgToTF(quaternion2, rot2);
    return rot1.angleShortestPath(rot2);
  }

  const geometry_msgs::PoseStamped GetRobotMapPose() {
    UpdateRobotPose();
    return robot_map_pose_;
  }

  const std::shared_ptr<CostMap> GetCostMap(){
    return costmap_ptr_;
  }

  const CostMap2D* GetCostMap2D() {
    return costmap_2d_;
  }

  const unsigned char* GetCharMap() {
    return charmap_;
  }

private:
  void UpdateRobotPose() {
    tf::Stamped<tf::Pose> robot_tf_pose;
    robot_tf_pose.setIdentity();

    robot_tf_pose.frame_id_ = "base_link";
    robot_tf_pose.stamp_ = ros::Time();
    try {
      geometry_msgs::PoseStamped robot_pose;
      tf::poseStampedTFToMsg(robot_tf_pose, robot_pose);
      tf_ptr_->transformPose("map", robot_pose, robot_map_pose_);
    }
    catch (tf::LookupException &ex) {
      ROS_ERROR("Transform Error looking up robot pose: %s", ex.what());
    }
  }

  //! id color info
  int my_id = 1;//1-red1;2-red2;3-blue1;4-blue2;
  int my_color = 1;//1-red;2-blue;

  //! tf
  std::shared_ptr<tf::TransformListener> tf_ptr_;

  //! Enenmy detection
  ros::Subscriber enemy_sub_;
  ros::Publisher  aim_target_pub_;

  //! Goal info
  geometry_msgs::PoseStamped goal_;
  bool new_goal_;

  //! Enemy info
  actionlib::SimpleActionClient<roborts_msgs::ArmorDetectionAction> armor_detection_actionlib_client_;
  roborts_msgs::ArmorDetectionGoal armor_detection_goal_;
  geometry_msgs::PoseStamped enemy_pose_;
  bool enemy_detected_;

  //! cost map
  std::shared_ptr<CostMap> costmap_ptr_;
  CostMap2D* costmap_2d_;
  unsigned char* charmap_;

  //! robot map pose
  geometry_msgs::PoseStamped robot_map_pose_;

  //! cmd_vel info
  ros::Subscriber cmd_vel_sub_;
  geometry_msgs::Twist current_cmd_vel_;

  //! odom info
  ros::Subscriber odom_sub_;
  nav_msgs::Odometry odom_;

  ros::Subscriber scan_sub_;
  sensor_msgs::LaserScan scan_;
  
  ros::Subscriber global_costmap_sub_;
  nav_msgs::OccupancyGrid global_costmap_;  

  //! referee system info
  ros::Subscriber game_result_sub_;
  roborts_msgs::GameResult game_result_;//Game result information.
  ros::Subscriber game_robot_bullet_sub_;
  roborts_msgs::GameRobotBullet game_robot_bullet_;//Game robot remaining bullet information of four robots.
  ros::Subscriber game_robot_hp_sub_;
  roborts_msgs::GameRobotHP game_robot_hp_;//Game robot health point information of four robots.
  ros::Subscriber game_status_sub_;
  roborts_msgs::GameStatus game_status_;//Game status with its remaining time information.
  ros::Subscriber game_zone_array_status_sub_;
  roborts_msgs::GameZoneArray game_zone_array_status_;//Six game zone status with its type and activation flag.
  ros::Subscriber robot_damage_sub_;
  roborts_msgs::RobotDamage robot_damage_;//Robot damage type information and damage source (armor id) if damage type is armor.
  ros::Subscriber robot_heat_sub_;
  roborts_msgs::RobotHeat robot_heat_;//Chassis voltage, current, power information and shooter heat information
  ros::Subscriber robot_shoot_sub_;
  roborts_msgs::RobotShoot robot_shoot_;//Robot shoot frequency and speed information
  ros::Subscriber robot_status_sub_;
  roborts_msgs::RobotStatus robot_status_;//Robot status information

  //! camera info
  ros::Subscriber camera_sub_;
  roborts_msgs::multi_car camera_info_;
  std_msgs::Time camera_find_time_;
  geometry_msgs::PoseStamped near_posestamped;

  //! under_attack info
  std_msgs::Float64 enemy_attack_me;
  bool under_attack_sign = false;
  nav_msgs::Odometry attacked_odom_;

  //! gyro info
  ros::Subscriber gimbal_angle_sub_;
  double yaw_angle_;//这个数据记录了云台的角位置, 对地面参考  
    
  //! visual detection info
  ros::Subscriber visual_detection_sub_;
  roborts_msgs::visual_detection mess_all_;
  bool if_shoot_;
  bool if_enemy_;
  std_msgs::Time last_find_time_;
  roborts_msgs::GimbalAngle gimbalangle_;
  geometry_msgs::PoseStamped aim_position_;//接受传输的信息

  //! buff info
  bool buffopeninfo[6];//数组的下标为buff类型
  /*顺序为: RED_HP_RECOVERY=1
           RED_BULLET_SUPPLY=2
           BLUE_HP_RECOVERY=3
           BLUE_BULLET_SUPPLY=4
           DISABLE_SHOOTING=5
           DISABLE_MOVEMENT=6*/
  int buffpositioninfo[6];//数组的下标为buff类型,元素数值为该buff所在地点
  ros::Publisher current_buff_open_pub_;
  roborts_costmap::Nlayer buff_open_pub_info;//数组下标为buff区序号,元素数值1-升起空气墙
  geometry_msgs::PoseStamped buff_position_[6];//6个buff区域所在地图坐标

  //! tcp info
  ros::Publisher robot_send_pub_;
  ros::Publisher air_block_pub_;
  ros::Subscriber robot_recv_sub_;
  roborts_msgs::multi_car tcp_car_info_;
  std_msgs::Time last_tcp_time_;
  
  ros::ServiceClient shoot_client;
  roborts_decision::DecisionConfig decision_config;

  IMUBuff* imu_buff;

  //condition bools
  bool chase_condition_track_;
  bool prepare_action_;
  ros::Time prepare_time_;
  bool snake_chase;
};
} //namespace roborts_decision
#endif //ROBORTS_DECISION_BLACKBOARD_H

